from geometry_msgs.msg import PoseStamped   #地理信息下的位置信息
from nav2_simple_commander.robot_navigator import BasicNavigator #navigation2的简单命令下的机器人导航下的基础导航
import rclpy    #ros client library for python


def main(args=None):
    rclpy.init(args=args)
    nav=BasicNavigator()
    goal_poses=[]
    goal_pose1=PoseStamped()
    goal_pose1.header.frame_id="map"
    goal_pose1.header.stamp=nav.get_clock().now().to_msg()
    goal_pose1.pose.position.x=-1.0
    goal_pose1.pose.position.y=-2.3
    goal_pose1.pose.orientation.w=1.0
    goal_poses.append(goal_pose1)
    goal_pose2=PoseStamped()
    goal_pose2.header.frame_id="map"
    goal_pose2.header.stamp=nav.get_clock().now().to_msg()
    goal_pose2.pose.position.x=1.0
    goal_pose2.pose.position.y=-2.3
    goal_pose2.pose.orientation.w=1.0
    goal_poses.append(goal_pose2)
    goal_pose3=PoseStamped()
    goal_pose3.header.frame_id="map"
    goal_pose3.header.stamp=nav.get_clock().now().to_msg()
    goal_pose3.pose.position.x=0.0
    goal_pose3.pose.position.y=0.0
    goal_pose3.pose.orientation.w=1.0
    goal_poses.append(goal_pose3)

    nav.followWaypoints(goal_poses)#启动路点跟随

    while not nav.isTaskComplete():
        feedback=nav.getFeedback()
        nav.get_logger().info(f'路点编号：{feedback.current_waypoint}')
    result=nav.getResult()
    nav.get_logger().info(f'导航结果：{str(result)}')
    # rclpy.spin(nav)
    # nav.destroy_node()
    # rclpy.shutdown()